import numpy as np
from  draw_map import visualize_line, visualize_line1, visualize_video, draw_rotate_car_position
import math

class PointsAttribue:
    def __init__(self, fuse_xyz, fuse_rpy):
        self.x, self.y, self.z = fuse_xyz
        self.r, self.p, self.yaw = fuse_rpy
        self.size = 0.05
        self.MAP_SIZE = 2
        self.global_gird_x = int(self.x / self.size)
        self.global_gird_y = int(self.y / self.size)
        self.CENTER = (50,50)
        self.OBTS_WORLD = []
        self.OBTS_GRID = []
        self.OBTS_GLOBAL_GRID = []


    def get_tansform_points(self, points, fusepsoe_xyz):
        for point in points:
            px,py,pz = point
            
            #增加相对后轮距离
            px = px + 0.513
            if not (-self.MAP_SIZE <= px <= self.MAP_SIZE and -self.MAP_SIZE <= py <= self.MAP_SIZE):
                continue
            # if pz < 0.05:
            #     continue
            pp = (px,py,pz)
            obts_world, obts_grid, obts_global_grid = self.transform_point_matrix(pp, fusepsoe_xyz)
            self.OBTS_GLOBAL_GRID.append(obts_global_grid)
            self.OBTS_GRID.append(obts_grid)
            self.OBTS_WORLD.append(obts_world)

    
    def get_obts_info(self):
        # 返回障碍物点的 世界坐标，局部地图坐标， 全局地图坐标
        return self.OBTS_WORLD, self.OBTS_GRID, self.OBTS_GLOBAL_GRID



    
    #相对坐标转 世界坐标 平移量 和栅格数目
    def transform_point_matrix(self, local_points, fusepsoe_xyz):
        yaw = self.yaw
        x_local, y_local, _ = local_points
        x, y, z = fusepsoe_xyz
        # 创建旋转矩阵
        rotation_matrix = np.array([
            [np.cos(yaw), -np.sin(yaw)],
            [np.sin(yaw), np.cos(yaw)]
        ])

        # 创建局部坐标的列向量
        local_point = np.array([x_local, y_local])

        # 应用旋转矩阵
        rotated_point = np.dot(rotation_matrix, local_point)

        grid_size = self.size
        # 在获得 grid_x, grid_y

        # 障碍物点平移到世界坐标系
        x_world = self.x + rotated_point[0]
        y_world = self.y + rotated_point[1]

        # xx = int(self.x / self.size)
        # yy = int(self.y / self.size)

        # grid_x = self.CENTER[0] + int(rotated_point[0] / grid_size)
        # grid_y = self.CENTER[1] + int(rotated_point[1] / grid_size)
        global_obts_grid_x = int(x_world / self.size)
        global_obts_grid_y = int(y_world / self.size)

        centerx = int( x / grid_size)
        centery = int( y / grid_size)

        grid_x = global_obts_grid_x - centerx + self.CENTER[0]
        grid_y = global_obts_grid_y - centery + self.CENTER[1]

        # return x_world, y_world, rotated_point[0], rotated_point[1],  grid_x, grid_y
        return (x_world, y_world), (grid_x, grid_y), (global_obts_grid_x, global_obts_grid_y)
    


    
    def get_local_relative_xy(self, cur_postion_grid_xyz):
        x, y, z, = cur_postion_grid_xyz

        delta_x = self.global_gird_x - x
        delta_y = self.global_gird_y - y

        return (delta_x, delta_y, 0)

    def get_global_xyz(self):
        return(self.x, self.y, self.z)
    
    def get_global_grid_xy(self):
        return (self.global_gird_x, self.global_gird_y, 0)
  
class BuildGridMap:
    def __init__(self, lidar_data):
        self.lidar_data = lidar_data
        self.global_size = (0, 0)  # 初始化全局尺寸
        self.x_min, self.y_min, self.x_max, self.y_max = 0,0,0,0
        self.grid_size = 0.05
        self.calculate_map_grid_size()
        self.GLOBAL_MAP = np.full(self.global_size, 0.5)  # 初始占用概率为 0.5
        print(f'Global size {self.global_size}')

    def calculate_map_grid_size(self):
        # 提取所有 fusepose 的 x, y
        fusepose_xy = np.array([frame['fusepose'][:2] for frame in self.lidar_data if 'fusepose' in frame])
        
        # 计算 x 和 y 的最小值和最大值
        x_min, y_min = fusepose_xy[:, 0].min(), fusepose_xy[:, 1].min()
        x_max, y_max = fusepose_xy[:, 0].max(), fusepose_xy[:, 1].max()
        self.x_min, self.y_min, self.x_max, self.y_max = x_min, y_min, x_max, y_max
        # 打印结果
        print(f"Fusepose x 范围: 最小值 = {x_min}, 最大值 = {x_max}")
        print(f"Fusepose y 范围: 最小值 = {y_min}, 最大值 = {y_max}")

        # 计算全局尺寸
        global_x = int((x_max - x_min) / self.grid_size  )
        global_y = int((y_max - y_min) / self.grid_size  )
        self.global_size = (global_x, global_y)
        self.global_size = (round(self.global_size[0], -1), round(self.global_size[1], -1))

    def get_global_map(self):
        return self.GLOBAL_MAP
    
    def get_global_size(self):
        return self.global_size
    
    def xyz2xy(self, x,y,z):
        x1 = (x - self.x_min) / self.global_size
        y1 = (y - self.y_min) / self.global_size
        x1 = int(x1)
        y1 = int(y1)
    
        return x1,y1


class updateMap():
    def __init__(self, LOCAL_MAP, GLOBAL_MAP, local_map_size = (100,100)):
        self.EMPTY_LOCAL_MAP = np.full(local_map_size, 0.5)
        self.BEFORE_LOCAL_MAP = LOCAL_MAP
        self.UPDATE_LOCAL_MAP = LOCAL_MAP
        self.global_map = GLOBAL_MAP

        self.SIZE = 2
        self.CENTER = 50, 50
        self.MINI_GRID = 0.05

        self.obts_world_sets = []
        self.obts_global_grid_sets = []
        self.obts_grid_sets = []
        self.LOCAL_MAP_SETS = []
        self.fuse_xyz_sets = []
        self.fuse_rpy_sets = []

    def jude_obts_in(self, obts_postion, fuse_postiion):
        x0,y0,_ = fuse_postiion
        x1,y1 = x0 - self.SIZE, y0 - self.SIZE
        x2,y2 = x0 + self.SIZE, y0 + self.SIZE
        x,y = obts_postion
        if min(x1, x2) <= x <= max(x1, x2) and min(y1, y2) <= y <= max(y1, y2):
            return True
    
        return False
        # pass


    def calac_r_theta(self, obts_potion, camera_postion):

        x,y = obts_potion # 障碍物
        x0,y0 = camera_postion # 车头
        # x1,y1 = x0 - int(self.SIZE / self.MINI_GRID), y0 - int(self.SIZE / self.MINI_GRID)
        # x2,y2 = x0 + int(self.SIZE / self.MINI_GRID), y0 + int(self.SIZE / self.MINI_GRID)


        r, theta = self.calculate_polar_coordinates(x0, y0, x, y)
        # return r, theta

        visuon_bny = 55 / 57.13
        if r < 30 and (theta < visuon_bny  and theta > visuon_bny):
            return True
        
        return False



    def calculate_polar_coordinates(self, x, y, x1, y1):
        # 计算半径 r
        r = math.sqrt((x1 - x)**2 + (y1 - y)**2)
        
        # 计算角度 theta
        theta = math.atan2(y1 - y, x1 - x)
        
        return r, theta

    def jude_obsts_gloab_in(self, obts_postion, fuse_postion):
        x,y = obts_postion # 障碍物
        x0,y0,_ = fuse_postion
        x1,y1 = x0 - int(self.SIZE / self.MINI_GRID), y0 - int(self.SIZE / self.MINI_GRID)
        x2,y2 = x0 + int(self.SIZE / self.MINI_GRID), y0 + int(self.SIZE / self.MINI_GRID)

        if min(x1, x2) <= x <= max(x1, x2) and min(y1, y2) <= y <= max(y1, y2):
            return True
    
        return False



    def obts_grid_xy_2_new_xy(self, points, fusepsoe_xyz, fusepose_rpy, i):
        pass


    def world2gird(self, world_point, fusepose_xyz):
        x,y= world_point
        x1,y1,z1 = fusepose_xyz

        grid_x = self.CENTER[0] + (x - x1) / self.MINI_GRID
        grid_y = self.CENTER[0] + (y - y1) / self.MINI_GRID

        return int(grid_x), int(grid_y)
    

    def get_unique_data(self, data):
        # 用于存储已经看到的坐标
        seen = set()

        # 用于存储不重复坐标的下标
        unique_indexes = []

        # 遍历列表，记录第一次出现的坐标下标
        for index, coord in enumerate(data):
            if coord not in seen:
                seen.add(coord)
                unique_indexes.append(index)

        # print(unique_indexes)
        return unique_indexes


    def calc_in_visual(self, theta_radian, theta_degree, fusepose_rpy):
        pass
        
        _, _, yaw = fusepose_rpy
        Yaw = yaw * 57.14






    def calculate_angle(self, p1 , p2):
        """
        计算两点连线与 x 轴的夹角。
        
        参数:
            x1, y1: 第一个点的坐标
            x2, y2: 第二个点的坐标
        
        返回:
            (theta_radian, theta_degree): 夹角的弧度和角度
        """
        # 计算坐标差
        x1, y1 = p1
        x2, y2 = p2
        delta_x = x2 - x1
        delta_y = y2 - y1
        
        # # 计算夹角（弧度）
        # theta_radian = math.atan2(delta_y, delta_x)
        
        # # 弧度转换为角度
        # theta_degree = math.degrees(theta_radian)
        
        # return theta_radian, theta_degree

            # 计算径向距离 r
        r = math.sqrt((x2 - x1)**2 + (y2 - y1)**2)
        
        # 计算角度 theta（弧度制）
        theta_radian = math.atan2(y2 - y1, x2 - x1)
        
        # 将角度转换为角度制
        theta_degree = math.degrees(theta_radian)


        if  r < 20 and  (theta_degree < 65 and theta_degree > -65):
            return True
        
        return False


    def update_local_map(self, points, fusepsoe_xyz, fusepose_rpy, i):

        before_map = self.BEFORE_LOCAL_MAP  # 初始占用概率为 0.5
        update_map = np.full((100,100), 0.5)


        cur_postion = PointsAttribue(fusepsoe_xyz, fusepose_rpy)
        cur_global_grid = cur_postion.get_global_grid_xy()
        before_lobal_grid_xy_sets = []
        before_world_xy_sets = []
        befor_gird_xy_sets = []

        left_points, right_points, car_postion = self.visual_index_running_area(fusepose_rpy)
        if i == 0:
            pass

        else:
            # 上一帧中大于 0.5 的数据
            last_world_data = self.obts_world_sets[i-1]
            last_global_grid_data = self.obts_global_grid_sets[i-1]
            last_grid_data = self.obts_grid_sets[i-1]

            # 世界坐标  全局栅格坐标   栅格坐标
            for world_points, global_grid_data, grid_points in zip(last_world_data,last_global_grid_data,last_grid_data):
                old_grid_x, old_grid_y = grid_points

                # if self.jude_obts_in(world_points, fusepsoe_xyz) == False:
                # 判断全局障碍物点在当前坐标系下是不是可见区域
                if self.jude_obsts_gloab_in(global_grid_data, cur_global_grid) == False:
                # # 求全局
                # cur_x = global_grid_data[0] - cur_global_grid[0] + 50 # x的相对坐标
                # cur_y = global_grid_data[1] - cur_global_grid[1] + 50  # y的相对坐标
                # if self.calac_r_theta(car_postion, (cur_x, cur_y)) == False:
                    continue
                else:
                    # 相对当前位置的栅格坐标
                    # grid_xy =  list(global_grid_data) - list(cur_global_grid)
                    # grid_xy = subtract_tuples(coord1, coord2)
                    grid_x = global_grid_data[0] - cur_global_grid[0] + 50
                    grid_y = global_grid_data[1] - cur_global_grid[1] + 50
                    grid_xy = (grid_x, grid_y)
                    old_grid = (old_grid_x, old_grid_y)
                    update_map[grid_xy] = before_map[old_grid]

                    before_lobal_grid_xy_sets.append(global_grid_data)
                    before_world_xy_sets.append(world_points)
                    befor_gird_xy_sets.append(grid_xy)



        tmp_obts_world_set = []
        tmp_obts_global_grid_set = []
        tmp_obts_grid_set = []



        update_memory = np.full((100,100), 0)  # 初始占用概率为 0.5


        # update
        cur_postion.get_tansform_points(points, fusepsoe_xyz)
        obts_worlds, obts_grids, obts_global_grids = cur_postion.get_obts_info()

        index_tmp = self.get_unique_data(obts_global_grids)

        obts_worlds = [obts_worlds[i] for i in index_tmp]
        obts_grids = [obts_grids[i] for i in index_tmp]
        obts_global_grids = [obts_global_grids[i] for i in index_tmp]

        index_tmp1 = self.get_unique_data(obts_grids)
        obts_worlds = [obts_worlds[i] for i in index_tmp1]
        obts_grids = [obts_grids[i] for i in index_tmp1]
        obts_global_grids = [obts_global_grids[i] for i in index_tmp1]
        
        print(f'obts.len {obts_global_grids.__len__()}')
        print(f'before.len {before_lobal_grid_xy_sets.__len__()}')
        
        list1 = before_lobal_grid_xy_sets
        list2 = obts_global_grids
        
        # 找到两个列表中相同的坐标值
        common_coords = set(list1) & set(list2)

        # 分别获取相同坐标值在每个列表中的下标
        # before
        before_lists = [list1.index(coord) for coord in common_coords]
        now_lists = [list2.index(coord) for coord in common_coords]

        # 先遍历
        for i, global_grid_data in enumerate(obts_global_grids):
            tmp_grid = obts_grids[i]
            tmp_worlds = obts_worlds[i]
            tmp_ggrids = obts_global_grids[i]

            # 如果 现在出现的点在公共部分
            if i in now_lists:
                if update_memory[tmp_grid] == 0:
                    update_memory[tmp_grid] = 1
                    update_map[tmp_grid] = self.update_probability(update_map[tmp_grid], 0.8, 0.2)
                else:
                    continue

            # 如果这些点是新出现的
            else:
                if update_memory[tmp_grid] == 0:
                    update_memory[tmp_grid] = 1
                    old_prob = update_map[tmp_grid]
                    update_map[tmp_grid] = self.update_probability(old_prob, 0.7, 0.3)
                    # print(f'old {old_prob} update: {update_map[tmp_grid]}')
                else:
                    continue
            tmp_obts_world_set.append(tmp_worlds)
            tmp_obts_global_grid_set.append(tmp_ggrids)
            tmp_obts_grid_set.append(tmp_grid)

        # remaining
        for i, global_grid_data in enumerate(before_lobal_grid_xy_sets):
            # global grid-map coord
            tmp_grid = befor_gird_xy_sets[i]
            tmp_worlds = before_world_xy_sets[i]
            # local grid-map coord
            tmp_ggrids = before_lobal_grid_xy_sets[i]


            # 计算弧度
            x11, y11 = tmp_grid
            p1 = (x11,y11)
            p_base = car_postion
            # 说明不是共有部分障碍物的点
            if i not in before_lists:
                # 如果在视野内 刷新掉
                if  self.calculate_angle(p_base, p1) == True:
                    pass
                    update_map[tmp_grid] = self.update_probability(update_map[tmp_grid], 0.4, 0.6)
                
                # 保留
                else:
                    pass
                    
                if update_map[tmp_grid] <= 0.5:
                    update_map[tmp_grid] = 0.5
                else:
                    tmp_obts_world_set.append(tmp_worlds)
                    tmp_obts_global_grid_set.append(tmp_ggrids)
                    tmp_obts_grid_set.append(tmp_grid)

           


            # 如果 i 不在下标里
            # if i not in before_lists:
            #     # update_map[tmp_grid] = self.update_probability(update_map[tmp_grid], 0.45, 0.55)
            #     pass

            #     if update_map[tmp_grid] <= 0.5:
            #         update_map[tmp_grid] = 0.5
            #     else:
            #         tmp_obts_world_set.append(tmp_worlds)
            #         tmp_obts_global_grid_set.append(tmp_ggrids)
            #         tmp_obts_grid_set.append(tmp_grid)



        update_map[update_map < 0.5] = 0.5

        # 统计大于 0.5 的元素数量
        count_greater_than_0_5 = np.sum(update_map > 0.5)

        # 打印统计结果
        print(f"Number of elements greater than 0.5: {count_greater_than_0_5}")

        # 提取大于 0.5 的数值
        values_greater_than_0_5 = update_map[update_map > 0.5]

        # 打印这些数值
        print("Values greater than 0.5:")
        print(values_greater_than_0_5)
        # 获取所有大于0.5的元素的坐标
        rows, cols = np.where(update_map > 0.5)

        # 打印坐标和对应的值
        print("Values greater than 0.5:")
        for row, col in zip(rows, cols):
            print(f"Coordinate ({row}, {col}): {update_map[row, col]}")
        self.UPDATE_LOCAL_MAP = update_map
        self.BEFORE_LOCAL_MAP = update_map
        self.LOCAL_MAP_SETS.append(update_map)
        self.fuse_rpy_sets.append(fusepose_rpy)
        self.fuse_xyz_sets.append(fusepsoe_xyz)

        self.obts_world_sets.append(tmp_obts_world_set)
        self.obts_global_grid_sets.append(tmp_obts_global_grid_set)
        self.obts_grid_sets.append(tmp_obts_grid_set)


    def update_probability(self, P_O, P_Z_given_O, P_Z_given_not_O):
        P_Z = P_Z_given_O * P_O + P_Z_given_not_O * (1 - P_O)
        P_O_given_Z = (P_Z_given_O * P_O) / P_Z
        return P_O_given_Z
        # grid[center] = update_probability(grid[center], P_Z_given_O, P_Z_given_not_O)
    

    def update_global_map(self):
        pass


    def visual_index_local_map(self, i, conf=0.5):
        grid_map = self.LOCAL_MAP_SETS[i]
        fusepose_rpy = self.fuse_rpy_sets[i]
        # visualize_line(grid_map ,i)
        visualize_line1(grid_map , fusepose_rpy, i, conf)
        pass
    
    
    def isInTriangle(self, cam_postion, grid_xy, fusepose_rpy):
        cx, cy = cam_postion
        gx, gy = grid_xy
        _, _, yaw = fusepose_rpy


        # if cy == gy:
        #     if yaw < 
        # else:
        #     pass


    def visual_index_running_area(self, fusepose_rpy):
        # draw_rotate_car_position()
        left_points, right_points, cposition = draw_rotate_car_position((100,100), fusepose_rpy , car_size = (666, 472, 294, 513))
        return left_points, right_points, cposition
    

    def visual_index_video_map(self, i, conf=0.5):
        grid_map = self.LOCAL_MAP_SETS[i]
        fusepose_rpy = self.fuse_rpy_sets[i]
        # visualize_line(grid_map ,i)
        img = visualize_video(grid_map , fusepose_rpy, i, conf)
        
        return img 
    
    def get_map_xyz_rpy(self):
        # return self.LOCAL_MAP_SETS, self.xyz_sets, self.rpy_sets, self.points_sets, self.relateive_points_sets, self.world_points_sets
        pass



def update_probability(P_O, P_Z_given_O, P_Z_given_not_O):
    P_Z = P_Z_given_O * P_O + P_Z_given_not_O * (1 - P_O)
    P_O_given_Z = (P_Z_given_O * P_O) / P_Z
    return P_O_given_Z


if __name__ == '__main__':
    local_center = 0.88
    detect =  0.4
    no_detect = 1 - detect
    print(f'初始 {local_center}  {detect} {no_detect}')
    for i in range(5):
        local_center = update_probability(local_center, detect, no_detect)
        # print(f'{i} : {local_center}')
        print(f'{i} : {local_center:.2f}')


# 初始条件是 0.5 0.6 0.4
# 0 : 0.6
# 1 : 0.6923076923076923
# 2 : 0.7714285714285714
# 3 : 0.8350515463917525
# 4 : 0.8836363636363637

# 初始条件是 0.6 0.7 0.3
# 0 : 0.7777777777777777
# 1 : 0.8909090909090909
# 2 : 0.9501385041551247
# 3 : 0.9780040733197556
# 4 : 0.9904531793270079

# 初始条件是 0.9 0.3 0.7
